﻿#include "kalmanfilter.h"

KalmanFilter::KalmanFilter()
{
    initNLOS();
    initRtTracing();
    initBlend();
}

KalmanFilter::~KalmanFilter() = default;

/**                                    抑制NLOS(非视距误差)的卡尔曼滤波算法参数初始化
* @brief
* @param      kf：指向卡尔曼滤波中间变量的结构体
* @retval     None
*/
void KalmanFilter::initNLOS()
{
    nlos_p_.dt = 0.1f;
    nlos_p_.Q_Dr = 0.0025f;
    nlos_p_.Q_r = 0.01f;
    nlos_p_.R = 0.0225f;
    nlos_p_.rk = -1.0f;

    nlos_p_.P[0][0] = 1.0f;
    nlos_p_.P[0][1] = 0.0f;
    nlos_p_.P[1][0] = 0.0f;
    nlos_p_.P[1][1] = 1.0f;

    nlos_p_.ErrCount = 0;
}

/**                                    抑制NLOS(非视距误差)的卡尔曼滤波算法
* @brief      设置ek值为过滤阀门，滤除过大的数据
* @param      kf：指向卡尔曼滤波中间变量的结构体
* @param      Zk：测量值
* @retval     result：计算得出的估计值指针
*/
void KalmanFilter::NLOS(int Zk, int* result)
{
    /* X(k|k-1) = A * X(k-1|k-1) + B * u(k) */
    nlos_p_.rk_ = nlos_p_.rk + nlos_p_.d_rk * nlos_p_.dt;

    /* P(k|k-1) = A * P(k-1|k-1) * AT + Q */
    nlos_p_.AP_AT[0] = (nlos_p_.P[0][1] + nlos_p_.P[1][0]) * nlos_p_.dt + nlos_p_.P[1][1] * nlos_p_.dt * nlos_p_.dt + nlos_p_.Q_Dr;
    nlos_p_.AP_AT[1] = nlos_p_.P[1][1] * nlos_p_.dt;
    nlos_p_.AP_AT[2] = nlos_p_.P[1][1] * nlos_p_.dt;
    nlos_p_.AP_AT[3] = nlos_p_.Q_Dr;

    nlos_p_.P[0][0] += nlos_p_.AP_AT[0];
    nlos_p_.P[0][1] += nlos_p_.AP_AT[1];
    nlos_p_.P[1][0] += nlos_p_.AP_AT[2];
    nlos_p_.P[1][1] += nlos_p_.AP_AT[3];

    /* Kg(k) = P(k|k-1) * HT / (H * P(k|k-1) * HT + R) */
    nlos_p_.E = nlos_p_.P[0][0] + nlos_p_.R;
    nlos_p_.K_a = nlos_p_.P[0][0] / nlos_p_.E;
    nlos_p_.K_b = nlos_p_.P[1][0] / nlos_p_.E;

    nlos_p_.ek = Zk - nlos_p_.rk_;

    if ((nlos_p_.ek > 1000 || nlos_p_.ek < -500) && nlos_p_.rk > 0) {
        nlos_p_.K_a = 0;
        nlos_p_.K_b = 0;
        nlos_p_.ErrCount++;
    } else
        nlos_p_.ErrCount = 0;

    /* X(k|k) = X(k|k-1) + Kg(k) * (Z(k) - H * X(k|k-1)) */
    nlos_p_.rk = nlos_p_.rk_ + nlos_p_.K_a * nlos_p_.ek;
    nlos_p_.d_rk = nlos_p_.d_rk_ + nlos_p_.K_b * nlos_p_.ek;

    *result = static_cast<int>(nlos_p_.rk);

    /* P(k|k) = (I - Kg(k) * H) * P(k|k-1) */
    nlos_p_.KkHPk_a = nlos_p_.P[0][0];
    nlos_p_.KkHPk_b = nlos_p_.P[0][1];
    nlos_p_.P[0][0] -= nlos_p_.K_a * nlos_p_.KkHPk_a;
    nlos_p_.P[0][1] -= nlos_p_.K_a * nlos_p_.KkHPk_b;
    nlos_p_.P[1][0] -= nlos_p_.K_b * nlos_p_.KkHPk_a;
    nlos_p_.P[1][1] -= nlos_p_.K_b * nlos_p_.KkHPk_b;
}

/**                                    实时跟踪的卡尔曼滤波算法参数初始化
* @brief
* @param      kf：指向卡尔曼滤波中间变量的结构体
* @retval     None
*/
void KalmanFilter::initRtTracing()
{
    rt_p_.dt = 0.1f;
    rt_p_.dt_2 = 0.01f;
    rt_p_.Q_x = 0.0049f;
    rt_p_.Q_y = 0.0049f;
    rt_p_.R_x = 0.0225f;
    rt_p_.R_y = 0.0225f;
    rt_p_.xk = -1.0f;
    rt_p_.yk = -1.0f;

    rt_p_.P[0][0] = 1.0f;
    rt_p_.P[0][1] = 0.0f;
    rt_p_.P[0][2] = 0.0f;
    rt_p_.P[0][3] = 0.0f;
    rt_p_.P[1][0] = 0.0f;
    rt_p_.P[1][1] = 1.0f;
    rt_p_.P[1][2] = 0.0f;
    rt_p_.P[1][3] = 0.0f;
    rt_p_.P[2][0] = 0.0f;
    rt_p_.P[2][1] = 0.0f;
    rt_p_.P[2][2] = 1.0f;
    rt_p_.P[2][3] = 0.0f;
    rt_p_.P[3][0] = 0.0f;
    rt_p_.P[3][1] = 0.0f;
    rt_p_.P[3][2] = 0.0f;
    rt_p_.P[3][3] = 1.0f;

    rt_p_.ErrCount = 0;
}

/**                                    实时跟踪的卡尔曼滤波算法
* @brief
* @param      kf：指向卡尔曼滤波中间变量的结构体
* @param      Zk：测量值
* @retval     result：计算得出的估计值指针
*/
void KalmanFilter::rtTracing(const Point& input, Point* result)
{
    /* Xk` = A * Xk + B * u(k) */
    rt_p_.xk_ = rt_p_.xk + rt_p_.Vxk * rt_p_.dt;
    rt_p_.yk_ = rt_p_.yk + rt_p_.Vyk * rt_p_.dt;

    /* Pk` = A * Pk-1 * AT + Q */
    rt_p_.AP_AT[0] = (rt_p_.P[2][0] + rt_p_.P[0][2]) * rt_p_.dt + rt_p_.P[2][2] * rt_p_.dt_2 + rt_p_.Q_x;
    rt_p_.AP_AT[1] = (rt_p_.P[2][1] + rt_p_.P[0][3]) * rt_p_.dt + rt_p_.P[2][3] * rt_p_.dt_2;
    rt_p_.AP_AT[2] = rt_p_.P[2][2] * rt_p_.dt;
    rt_p_.AP_AT[3] = rt_p_.P[2][3] * rt_p_.dt;
    rt_p_.AP_AT[4] = (rt_p_.P[3][0] + rt_p_.P[1][2]) * rt_p_.dt + rt_p_.P[3][2] * rt_p_.dt_2;
    rt_p_.AP_AT[5] = (rt_p_.P[3][1] + rt_p_.P[1][3]) * rt_p_.dt + rt_p_.P[3][3] * rt_p_.dt_2 + rt_p_.Q_y;
    rt_p_.AP_AT[6] = rt_p_.P[3][2] * rt_p_.dt;
    rt_p_.AP_AT[7] = rt_p_.P[3][3] * rt_p_.dt;
    rt_p_.AP_AT[8] = rt_p_.P[2][2] * rt_p_.dt;
    rt_p_.AP_AT[9] = rt_p_.P[2][3] * rt_p_.dt;
    rt_p_.AP_AT[10] = rt_p_.Q_Vx;
    rt_p_.AP_AT[12] = rt_p_.P[3][2] * rt_p_.dt;
    rt_p_.AP_AT[13] = rt_p_.P[3][3] * rt_p_.dt;
    rt_p_.AP_AT[15] = rt_p_.Q_Vy;

    rt_p_.P[0][0] += rt_p_.AP_AT[0];
    rt_p_.P[0][1] += rt_p_.AP_AT[1];
    rt_p_.P[0][2] += rt_p_.AP_AT[2];
    rt_p_.P[0][3] += rt_p_.AP_AT[3];
    rt_p_.P[1][0] += rt_p_.AP_AT[4];
    rt_p_.P[1][1] += rt_p_.AP_AT[5];
    rt_p_.P[1][2] += rt_p_.AP_AT[6];
    rt_p_.P[1][3] += rt_p_.AP_AT[7];
    rt_p_.P[2][0] += rt_p_.AP_AT[8];
    rt_p_.P[2][1] += rt_p_.AP_AT[9];
    rt_p_.P[2][2] += rt_p_.AP_AT[10];
    rt_p_.P[2][3] += rt_p_.AP_AT[11];
    rt_p_.P[3][0] += rt_p_.AP_AT[12];
    rt_p_.P[3][1] += rt_p_.AP_AT[13];
    rt_p_.P[3][2] += rt_p_.AP_AT[14];
    rt_p_.P[3][3] += rt_p_.AP_AT[15];

    /* Kk = Pk` * HT / (H * Pk` * HT + R) */
    rt_p_.detA = (rt_p_.P[0][0] + rt_p_.R_x) * (rt_p_.P[1][1] + rt_p_.R_y) - rt_p_.P[0][1] * rt_p_.P[1][0];
    rt_p_.aPlusR_xDivdetA = (rt_p_.P[0][0] + rt_p_.R_x) / rt_p_.detA;
    rt_p_.eDivdetA = rt_p_.P[1][0] / rt_p_.detA;
    rt_p_.bDivdetA = rt_p_.P[0][1] / rt_p_.detA;
    rt_p_.fPlusR_yDivdetA = (rt_p_.P[1][1] + rt_p_.R_y) / rt_p_.detA;
    rt_p_.K[0][0] = rt_p_.P[0][0] * rt_p_.fPlusR_yDivdetA - rt_p_.P[0][1] * rt_p_.eDivdetA;
    rt_p_.K[0][1] = -rt_p_.P[0][0] * rt_p_.bDivdetA + rt_p_.P[0][1] * rt_p_.aPlusR_xDivdetA;
    rt_p_.K[1][0] = rt_p_.P[1][0] * rt_p_.fPlusR_yDivdetA - rt_p_.P[1][1] * rt_p_.eDivdetA;
    rt_p_.K[1][1] = -rt_p_.P[1][0] * rt_p_.bDivdetA + rt_p_.P[1][1] * rt_p_.aPlusR_xDivdetA;
    rt_p_.K[2][0] = rt_p_.P[2][0] * rt_p_.fPlusR_yDivdetA - rt_p_.P[2][1] * rt_p_.eDivdetA;
    rt_p_.K[2][1] = -rt_p_.P[2][0] * rt_p_.bDivdetA + rt_p_.P[2][1] * rt_p_.aPlusR_xDivdetA;
    rt_p_.K[3][0] = rt_p_.P[3][0] * rt_p_.fPlusR_yDivdetA - rt_p_.P[3][1] * rt_p_.eDivdetA;
    rt_p_.K[3][1] = -rt_p_.P[3][0] * rt_p_.bDivdetA + rt_p_.P[3][1] * rt_p_.aPlusR_xDivdetA;

    /* Xk = Xk` + Kk * (Zk - H * Xk`)   ek = Zk - H * Xk`  */
    rt_p_.ek[0] = static_cast<float>(input.x()) - rt_p_.xk_;
    rt_p_.ek[1] = static_cast<float>(input.y()) - rt_p_.yk_;

    if ((rt_p_.ek[0] > 200 || rt_p_.ek[0] < -200 || rt_p_.ek[1] > 200 || rt_p_.ek[1] < -200)
        && (rt_p_.xk > 0 && rt_p_.yk > 0)) {
        rt_p_.K[0][0] = 0;
        rt_p_.K[0][1] = 0;
        rt_p_.K[1][0] = 0;
        rt_p_.K[1][1] = 0;
        rt_p_.K[2][0] = 0;
        rt_p_.K[2][1] = 0;
        rt_p_.K[3][0] = 0;
        rt_p_.K[3][1] = 0;
        rt_p_.ErrCount++;
    } else
        rt_p_.ErrCount = 0;

    //qDebug() << "errCount = " << rt_p_.ErrCount;

    rt_p_.xk = rt_p_.xk_ + rt_p_.K[0][0] * rt_p_.ek[0] + rt_p_.K[0][1] * rt_p_.ek[1];
    rt_p_.yk = rt_p_.yk_ + rt_p_.K[1][0] * rt_p_.ek[0] + rt_p_.K[1][1] * rt_p_.ek[1];
    rt_p_.Vxk_ = rt_p_.Vxk + rt_p_.K[2][0] * rt_p_.ek[0] + rt_p_.K[2][1] * rt_p_.ek[1];
    rt_p_.Vyk_ = rt_p_.Vyk + rt_p_.K[3][0] * rt_p_.ek[0] + rt_p_.K[3][1] * rt_p_.ek[1];

    result->setX(static_cast<double>(rt_p_.xk));
    result->setY(static_cast<double>(rt_p_.yk));

    /* Pk = (I - Kk * H) * Pk` = Pk` - Kk * H *Pk` */
    rt_p_.KkHPk_[0][0] = rt_p_.K[0][0] * rt_p_.P[0][0] + rt_p_.K[0][1] * rt_p_.P[1][0];
    rt_p_.KkHPk_[0][1] = rt_p_.K[0][0] * rt_p_.P[0][1] + rt_p_.K[0][1] * rt_p_.P[1][1];
    rt_p_.KkHPk_[0][2] = rt_p_.K[0][0] * rt_p_.P[0][2] + rt_p_.K[0][1] * rt_p_.P[1][2];
    rt_p_.KkHPk_[0][3] = rt_p_.K[0][0] * rt_p_.P[0][3] + rt_p_.K[0][1] * rt_p_.P[1][3];
    rt_p_.KkHPk_[1][0] = rt_p_.K[1][0] * rt_p_.P[0][0] + rt_p_.K[1][1] * rt_p_.P[1][0];
    rt_p_.KkHPk_[1][1] = rt_p_.K[1][0] * rt_p_.P[0][1] + rt_p_.K[1][1] * rt_p_.P[1][1];
    rt_p_.KkHPk_[1][2] = rt_p_.K[1][0] * rt_p_.P[0][2] + rt_p_.K[1][1] * rt_p_.P[1][2];
    rt_p_.KkHPk_[1][3] = rt_p_.K[1][0] * rt_p_.P[0][3] + rt_p_.K[1][1] * rt_p_.P[1][3];
    rt_p_.KkHPk_[2][0] = rt_p_.K[2][0] * rt_p_.P[0][0] + rt_p_.K[2][1] * rt_p_.P[1][0];
    rt_p_.KkHPk_[2][1] = rt_p_.K[2][0] * rt_p_.P[0][1] + rt_p_.K[2][1] * rt_p_.P[1][1];
    rt_p_.KkHPk_[2][2] = rt_p_.K[2][0] * rt_p_.P[0][2] + rt_p_.K[2][1] * rt_p_.P[1][2];
    rt_p_.KkHPk_[2][3] = rt_p_.K[2][0] * rt_p_.P[0][3] + rt_p_.K[2][1] * rt_p_.P[1][3];
    rt_p_.KkHPk_[3][0] = rt_p_.K[3][0] * rt_p_.P[0][0] + rt_p_.K[3][1] * rt_p_.P[1][0];
    rt_p_.KkHPk_[3][1] = rt_p_.K[3][0] * rt_p_.P[0][1] + rt_p_.K[3][1] * rt_p_.P[1][1];
    rt_p_.KkHPk_[3][2] = rt_p_.K[3][0] * rt_p_.P[0][2] + rt_p_.K[3][1] * rt_p_.P[1][2];
    rt_p_.KkHPk_[3][3] = rt_p_.K[3][0] * rt_p_.P[0][3] + rt_p_.K[3][1] * rt_p_.P[1][3];

    rt_p_.P[0][0] -= rt_p_.KkHPk_[0][0];
    rt_p_.P[0][1] -= rt_p_.KkHPk_[0][1];
    rt_p_.P[0][2] -= rt_p_.KkHPk_[0][2];
    rt_p_.P[0][3] -= rt_p_.KkHPk_[0][3];
    rt_p_.P[1][0] -= rt_p_.KkHPk_[1][0];
    rt_p_.P[1][1] -= rt_p_.KkHPk_[1][1];
    rt_p_.P[1][2] -= rt_p_.KkHPk_[1][2];
    rt_p_.P[1][3] -= rt_p_.KkHPk_[1][3];
    rt_p_.P[2][0] -= rt_p_.KkHPk_[2][0];
    rt_p_.P[2][1] -= rt_p_.KkHPk_[2][1];
    rt_p_.P[2][2] -= rt_p_.KkHPk_[2][2];
    rt_p_.P[2][3] -= rt_p_.KkHPk_[2][3];
    rt_p_.P[3][0] -= rt_p_.KkHPk_[3][0];
    rt_p_.P[3][1] -= rt_p_.KkHPk_[3][1];
    rt_p_.P[3][2] -= rt_p_.KkHPk_[3][2];
    rt_p_.P[3][3] -= rt_p_.KkHPk_[3][3];
}

/**                                    融合卡尔曼滤波算法参数初始化
* @brief      根据R/Q比重确定权重，值越大，越相信预测结果，反之亦然
* @param      kf：指向卡尔曼滤波中间变量的结构体
* @retval     None
*/
void KalmanFilter::initBlend()
{
    blend_p_.Q_x = 0.0025f;
    blend_p_.Q_y = 0.0025f;
    blend_p_.R_x = 0.0225f;
    blend_p_.R_y = 0.0225f;

    blend_p_.P[0][0] = 1.0f;
    blend_p_.P[1][0] = 0.0f;
    blend_p_.P[0][1] = 0.0f;
    blend_p_.P[1][1] = 1.0f;
}

/**                                    融合卡尔曼滤波算法参数初始化
* @brief      仅融合平面坐标(x,y),根据R/Q比重确定权重，值越大，越相信预测结果，反之亦然
* @param      kf：指向卡尔曼滤波中间变量的结构体
* @param      xk_：预测值->编码器计算值
* @param      Zk：测量值->飞测计算坐标值
* @retval     xk：计算得出的估计值指针
*/
void KalmanFilter::blend(const Point& predict, const Point& measure, Point* result)
{
    /* Xk` = A * Xk + B * u(k) */

    /* Pk` = A * Pk-1 * AT + Q */
    blend_p_.P[0][0] += blend_p_.Q_x;
    blend_p_.P[1][1] += blend_p_.Q_y;

    /* Zk = H*xk + vk        Kk = Pk` * HT / (H * Pk * HT + R)*/
    blend_p_.detA = (blend_p_.P[0][0] + blend_p_.R_x) * (blend_p_.P[1][1] + blend_p_.R_y) - blend_p_.P[0][1] * blend_p_.P[1][0];
    blend_p_.dPlusR_yDivdetA = (blend_p_.P[1][1] + blend_p_.R_y) / blend_p_.detA;
    blend_p_.bDivdetA = blend_p_.P[0][1] / blend_p_.detA;
    blend_p_.cDivdetA = blend_p_.P[1][0] / blend_p_.detA;
    blend_p_.aPlusR_xDivdetA = (blend_p_.P[1][1] + blend_p_.R_x) / blend_p_.detA;

    blend_p_.K[0][0] = blend_p_.P[0][0] * blend_p_.dPlusR_yDivdetA - blend_p_.P[0][1] * blend_p_.cDivdetA;
    blend_p_.K[0][1] = -(blend_p_.P[0][0] * blend_p_.bDivdetA) + blend_p_.P[0][1] * blend_p_.aPlusR_xDivdetA;
    blend_p_.K[1][0] = blend_p_.P[1][0] * blend_p_.dPlusR_yDivdetA - blend_p_.P[1][1] * blend_p_.cDivdetA;
    blend_p_.K[1][1] = -(blend_p_.P[1][0] * blend_p_.bDivdetA) + blend_p_.P[1][1] * blend_p_.aPlusR_xDivdetA;

    /* Xk = Xk` + Kk * (Zk - H * Xk`)    ek = Zk - H * Xk`*/
    blend_p_.ek[0] = static_cast<float>(measure.x() - predict.x());
    blend_p_.ek[1] = static_cast<float>(measure.y() - predict.y());

    //	if(blend_param_.ek[0] > 200 || blend_param_.ek[0] < -200 || blend_param_.ek[1] > 200 || blend_param_.ek[1] < -200)
    //	{
    //		blend_param_.K[0][0] = 0;
    //		blend_param_.K[0][1] = 0;
    //		blend_param_.K[1][0] = 0;
    //		blend_param_.K[1][1] = 0;
    //	}

    result->setX(predict.x() + static_cast<double>(blend_p_.K[0][0] * blend_p_.ek[0] + blend_p_.K[0][1] * blend_p_.ek[1]));
    result->setY(predict.y() + static_cast<double>(blend_p_.K[1][0] * blend_p_.ek[0] + blend_p_.K[1][1] * blend_p_.ek[1]));

    /* Pk = (I - Kk * H) * Pk` = Pk` - Kk * H *Pk` */
    blend_p_.KkHPk_[0][0] = blend_p_.K[0][0] * blend_p_.P[0][0] + blend_p_.K[0][1] * blend_p_.P[1][0];
    blend_p_.KkHPk_[0][1] = blend_p_.K[0][0] * blend_p_.P[0][1] + blend_p_.K[0][0] * blend_p_.P[1][1];
    blend_p_.KkHPk_[1][0] = blend_p_.K[1][0] * blend_p_.P[0][0] + blend_p_.K[1][1] * blend_p_.P[1][0];
    blend_p_.KkHPk_[1][1] = blend_p_.K[1][0] * blend_p_.P[0][1] + blend_p_.K[1][1] * blend_p_.P[1][1];

    blend_p_.P[0][0] -= blend_p_.KkHPk_[0][0];
    blend_p_.P[0][1] -= blend_p_.KkHPk_[0][1];
    blend_p_.P[1][0] -= blend_p_.KkHPk_[1][0];
    blend_p_.P[1][1] -= blend_p_.KkHPk_[1][1];
}
